/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the institute nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christoph Rösmann
 *********************************************************************/

#include <teb_local_planner/optimal_planner.h>
// g2o custom edges and vertices for the TEB planner
#include <teb_local_planner/g2o_types/edge_velocity.h>
#include <teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h>
#include <teb_local_planner/g2o_types/edge_acceleration.h>
#include <teb_local_planner/g2o_types/edge_kinematics.h>
#include <teb_local_planner/g2o_types/edge_time_optimal.h>
#include <teb_local_planner/g2o_types/edge_shortest_path.h>
#include <teb_local_planner/g2o_types/edge_obstacle.h>
#include <teb_local_planner/g2o_types/edge_dynamic_obstacle.h>
#include <teb_local_planner/g2o_types/edge_via_point.h>
#include <teb_local_planner/g2o_types/edge_prefer_rotdir.h>
#include <memory>
#include <limits>

namespace teb_local_planner
{

// ============== Implementation ===================
TebOptimalPlanner::TebOptimalPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL),
                                         prefer_rotdir_(RotType::none), initialized_(false), optimized_(false) {}

TebOptimalPlanner::TebOptimalPlanner(const TebConfig &cfg, ObstContainer *obstacles, TebVisualizationPtr visual,
                                     const ViaPointContainer *via_points)
{
    initialize(cfg, obstacles, visual, via_points);
}

TebOptimalPlanner::~TebOptimalPlanner()
{
    clearGraph();
    // free dynamically allocated memory
    //if (optimizer_)
    //  g2o::Factory::destroy();
    //g2o::OptimizationAlgorithmFactory::destroy();
    //g2o::HyperGraphActionLibrary::destroy();
}

void TebOptimalPlanner::initialize(const TebConfig &cfg, ObstContainer *obstacles, TebVisualizationPtr visual,
                                   const ViaPointContainer *via_points)
{
    // 初始化优化器 (设置求解器和block ordering) init optimizer (set solver and block ordering settings)
    optimizer_ = initOptimizer();

    cfg_ = &cfg;
    obstacles_ = obstacles;
    via_points_ = via_points;
    cost_ = HUGE_VAL;
    prefer_rotdir_ = RotType::none;
    setVisualization(visual);

    vel_start_.first = true;
    vel_start_.second.linear.x = 0;
    vel_start_.second.linear.y = 0;
    vel_start_.second.angular.z = 0;

    vel_goal_.first = true;
    vel_goal_.second.linear.x = 0;
    vel_goal_.second.linear.y = 0;
    vel_goal_.second.angular.z = 0;
    initialized_ = true;
}

void TebOptimalPlanner::setVisualization(TebVisualizationPtr visualization) { visualization_ = visualization; }

void TebOptimalPlanner::visualize()
{
    if (!visualization_) return;
    visualization_->publishLocalPlanAndPoses(teb_);
    if (teb_.sizePoses() > 0) visualization_->publishRobotFootprintModel(teb_.Pose(0), *cfg_->robot_model);
    if (cfg_->trajectory.publish_feedback) visualization_->publishFeedbackMessage(*this, *obstacles_);

}

// 基于g2o框架注册自定义顶点和边
void TebOptimalPlanner::registerG2OTypes()
{
    g2o::Factory *factory = g2o::Factory::instance();
    factory->registerType("VERTEX_POSE", new g2o::HyperGraphElementCreator<VertexPose>);
    factory->registerType("VERTEX_TIMEDIFF", new g2o::HyperGraphElementCreator<VertexTimeDiff>);
    factory->registerType("EDGE_TIME_OPTIMAL", new g2o::HyperGraphElementCreator<EdgeTimeOptimal>);
    factory->registerType("EDGE_SHORTEST_PATH", new g2o::HyperGraphElementCreator<EdgeShortestPath>);
    factory->registerType("EDGE_VELOCITY", new g2o::HyperGraphElementCreator<EdgeVelocity>);
    factory->registerType("EDGE_VELOCITY_HOLONOMIC", new g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>);
    factory->registerType("EDGE_ACCELERATION", new g2o::HyperGraphElementCreator<EdgeAcceleration>);
    factory->registerType("EDGE_ACCELERATION_START", new g2o::HyperGraphElementCreator<EdgeAccelerationStart>);
    factory->registerType("EDGE_ACCELERATION_GOAL", new g2o::HyperGraphElementCreator<EdgeAccelerationGoal>);
    factory->registerType("EDGE_ACCELERATION_HOLONOMIC", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>);
    factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>);
    factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>);
    factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>);
    factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>);
    factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeObstacle>);
    factory->registerType("EDGE_INFLATED_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeInflatedObstacle>);
    factory->registerType("EDGE_DYNAMIC_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeDynamicObstacle>);
    factory->registerType("EDGE_VIA_POINT", new g2o::HyperGraphElementCreator<EdgeViaPoint>);
    factory->registerType("EDGE_PREFER_ROTDIR", new g2o::HyperGraphElementCreator<EdgePreferRotDir>);
    return;
}

/* 初始化g2o优化器，求解器设置也在这里
* initialize g2o optimizer. Set solver settings here.
* Return: pointer to new SparseOptimizer Object.
*/
boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
{
    // 调用一次register_g2o_types，即使有多个TebOptimalPlanner实例（线程安全）
    // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
    static boost::once_flag flag = BOOST_ONCE_INIT;
    boost::call_once(&registerG2OTypes, flag);
    // 分配优化器
    // allocating the optimizer
    boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
    std::unique_ptr<TEBLinearSolver> linear_solver(new TEBLinearSolver()); // see typedef in optimization.h
    linear_solver->setBlockOrdering(true);
    std::unique_ptr<TEBBlockSolver> block_solver(new TEBBlockSolver(std::move(linear_solver)));
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
    optimizer->setAlgorithm(solver);
    optimizer->initMultiThreading(); // required for >Eigen 3.1
    return optimizer;
}

// 将轨迹优化问题构建成了一个g2o图优化问题并通过g2o中关于大规模稀疏矩阵的优化算法解决，
// 用到的超图构建(hyper-graph)，也就是将机器人位姿和时间差描述为顶点(vertex)（优化的对象），
// 目标函数以及约束函数被看做边(edges)，超图中每个约束都为一条edge，并且每条edge允许连接的vertex数目是不受限。
bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
                               double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
    if (!cfg_->optim.optimization_activate) return false;
    optimized_ = false;
    double weight_multiplier = 1.0;
    // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles
    //                (which leads to better results in terms of x-y-t homotopy planning).
    //                 however, we have not tested this mode intensively yet,
    //                 so we keep the legacy fast mode as default until we finish our tests.
    bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;
    for (int i = 0; i < iterations_outerloop; ++i)
    {
        if (cfg_->trajectory.teb_autosize)
        {
            //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
            teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples,
                            cfg_->trajectory.max_samples, fast_mode);
        }
        // step xx  构建超图
        bool success = buildGraph(weight_multiplier);
        if (!success)
        {
            clearGraph();
            return false;
        }
        // step xx  开始图优化
        success = optimizeGraph(iterations_innerloop, false);
        if (!success)
        {
            clearGraph();
            return false;
        }
        optimized_ = true;
        // step xx   compute_cost_afterwards 默认是false // compute cost vec only in the last iteration
        if (compute_cost_afterwards && i == iterations_outerloop - 1)
            computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
        clearGraph();
        weight_multiplier *= cfg_->optim.weight_adapt_factor;
    }
    return true;
}

void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist &vel_start)
{
    vel_start_.first = true;
    vel_start_.second.linear.x = vel_start.linear.x;
    vel_start_.second.linear.y = vel_start.linear.y;
    vel_start_.second.angular.z = vel_start.angular.z;
}

void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::Twist &vel_goal)
{
    vel_goal_.first = true;
    vel_goal_.second = vel_goal;
}

bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped> &initial_plan,
                             const geometry_msgs::Twist *start_vel, bool free_goal_vel)
{
    ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
    if (!teb_.isInit())
    {
        teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
                                  cfg_->trajectory.global_plan_overwrite_orientation,
                                  cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
    }
        // warm start 热启动
    else
    {
        PoseSE2 start_(initial_plan.front().pose);
        PoseSE2 goal_(initial_plan.back().pose);
        // actual warm start!
        if (teb_.sizePoses() > 0 &&
           (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist &&
           fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular)
            teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
            // goal too far away -> reinit 目标点太远，重新初始化
        else
        {
            ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
            teb_.clearTimedElasticBand();
            teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
                                      cfg_->trajectory.global_plan_overwrite_orientation,
                                      cfg_->trajectory.min_samples,
                                      cfg_->trajectory.allow_init_with_backwards_motion);
        }
    }
    if (start_vel) setVelocityStart(*start_vel);
    if (free_goal_vel) setVelocityGoalFree();
        // 再次激活并且用之前计算的速度 (如果没有什么修改，应该是零)
        // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
    else vel_goal_.first = true;
    // now optimize 开始热启动
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}


bool TebOptimalPlanner::plan(const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel,
                             bool free_goal_vel)
{
    PoseSE2 start_(start);
    PoseSE2 goal_(goal);
    return plan(start_, goal_, start_vel);
}

bool TebOptimalPlanner::plan(const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_vel,
                             bool free_goal_vel)
{
    ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
    if (!teb_.isInit())
    {
        // init trajectory 初始化轨迹
        teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples,
                                  cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization
    }
        // warm start 热启动
    else
    {
        // actual warm start!
        if (teb_.sizePoses() > 0
            && (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
            && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular)
            teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);
            // goal too far away -> reinit 目标点太远，重新初始化
        else
        {
            ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
            teb_.clearTimedElasticBand();
            teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples,
                                      cfg_->trajectory.allow_init_with_backwards_motion);
        }
    }
    if (start_vel) setVelocityStart(*start_vel);
    if (free_goal_vel) setVelocityGoalFree();
        // 再次激活并且用之前计算的速度 (如果没有什么修改，一般是零)
        // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
    else vel_goal_.first = true;
    // now optimize 开始优化
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}

bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
    if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
    {
        ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
        return false;
    }
    // 调用g20优化器的setComputeBatchStatistics函数，参数如果为true,为数据分配缓冲区
    optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable);
    // add TEB vertices 加入TEB顶点（位姿和时间差顶点）
    AddTEBVertices();
    // add Edges (local cost functions) 加入边（局部代价函数）
    if (cfg_->obstacles.legacy_obstacle_association) AddEdgesObstaclesLegacy(weight_multiplier);
    else AddEdgesObstacles(weight_multiplier);
    // 添加动态障碍物约束
    if (cfg_->obstacles.include_dynamic_obstacles) AddEdgesDynamicObstacles();
    AddEdgesViaPoints();     // 通过点约束
    AddEdgesVelocity();      // 速度约束
    AddEdgesAcceleration();  // 加速度约束
    AddEdgesTimeOptimal();   // 时间约束
    AddEdgesShortestPath();  // 最短路径约束
    // we have a differential drive robot   差速机器人运动学约束
    if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
        AddEdgesKinematicsDiffDrive();
        // we have a carlike robot since the turning radius is bounded from below. 类汽车机器人运动学约束，该类型受旋转半径的约束
    else AddEdgesKinematicsCarlike();
    AddEdgesPreferRotDir(); // 朝向约束
    if (cfg_->optim.weight_velocity_obstacle_ratio > 0) AddEdgesVelocityObstacleRatio();
    return true;
}

bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after)
{
    if (cfg_->robot.max_vel_x < 0.01)
    {
        ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted...");
        if (clear_after) clearGraph();
        return false;
    }
    if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
    {
        ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
        if (clear_after) clearGraph();
        return false;
    }
    optimizer_->setVerbose(cfg_->optim.optimization_verbose);  // 打印信息
    optimizer_->initializeOptimization();
    int iter = optimizer_->optimize(no_iterations);
    // Save Hessian for visualization
    //  g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast<g2o::OptimizationAlgorithmLevenberg*> (optimizer_->solver());
    //  lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt");
    if (!iter)
    {
        ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
        return false;
    }
    if (clear_after) clearGraph();
    return true;
}

void TebOptimalPlanner::clearGraph()
{
    // clear optimizer states 清除优化器的状态
    if (optimizer_)
    {
        // 清除所有的边，再清除所以顶点
        // we will delete all edges but keep the vertices.
        // before doing so, we will delete the link from the vertices to the edges.
        auto &vertices = optimizer_->vertices();
        for (auto &v : vertices) v.second->edges().clear();
        // 这样清理是有必要的，如果直接用optimizer->clear会删除指针对象（TEB的状态也也就没有了）
        // necessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!)
        optimizer_->vertices().clear();
        optimizer_->clear();
    }
}

void TebOptimalPlanner::AddTEBVertices()
{
    // add vertices to graph 添加顶点到图中
    ROS_DEBUG_COND(cfg_->optim.optimization_verbose, "Adding TEB vertices ...");
    unsigned int id_counter = 0; // used for vertices ids 用于顶点的索引
    obstacles_per_vertex_.resize(teb_.sizePoses());
    auto iter_obstacle = obstacles_per_vertex_.begin();
    for (int i = 0; i < teb_.sizePoses(); ++i)
    {
        teb_.PoseVertex(i)->setId(id_counter++); // 先记录PoseVertex的id
        optimizer_->addVertex(teb_.PoseVertex(i));  // 再添加位姿顶点
        if (teb_.sizeTimeDiffs() != 0 && i < teb_.sizeTimeDiffs())
        {
            teb_.TimeDiffVertex(i)->setId(id_counter++);
            optimizer_->addVertex(teb_.TimeDiffVertex(i));  // 添加时间差顶点
        }
        iter_obstacle->clear();
        (iter_obstacle++)->reserve(obstacles_->size());
    }
}

void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
{
    // if weight equals zero skip adding edges! 如果权重等于零则不添加该约束
    if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == nullptr) return;
    bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
    Eigen::Matrix<double, 2, 2> information_inflated;
    information_inflated(0, 0) = cfg_->optim.weight_obstacle * weight_multiplier;
    information_inflated(1, 1) = cfg_->optim.weight_inflation;
    information_inflated(0, 1) = information_inflated(1, 0) = 0;
    auto iter_obstacle = obstacles_per_vertex_.begin();
    // lambda表达式， 捕获inflated, &information, &information_inflated, this。函数参数为index和obstacle
    auto create_edge = [inflated, &information, &information_inflated, this](int index, const Obstacle *obstacle)
    {
        if (inflated)
        {
            EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle;
            dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index));
            dist_bandpt_obst->setInformation(information_inflated);
            dist_bandpt_obst->setParameters(*cfg_, obstacle);
            optimizer_->addEdge(dist_bandpt_obst);
        }
        else
        {
            EdgeObstacle *dist_bandpt_obst = new EdgeObstacle;
            dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index));
            dist_bandpt_obst->setInformation(information);
            dist_bandpt_obst->setParameters(*cfg_, obstacle);
            optimizer_->addEdge(dist_bandpt_obst);
        };
    };
    // 迭代所有的teb点，如果不创建EdgeVelocityObstacleRatio的边，跳过第一个和最后的点
    // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too
    const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0;
    for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i)
    {
        double left_min_dist = std::numeric_limits<double>::max();
        double right_min_dist = std::numeric_limits<double>::max();
        ObstaclePtr left_obstacle;
        ObstaclePtr right_obstacle;
        const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();
        // iterate obstacles 迭代障碍物
        for (const ObstaclePtr &obst : *obstacles_)
        {
            // we handle dynamic obstacles differently below 动态障碍物会被分别处理
            if (cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic()) continue;
            // calculate distance to robot model 计算到机器人模型的距离
            double dist = cfg_->robot_model->calculateDistance(teb_.Pose(i), obst.get());
            // force considering obstacle if really close to the current pose 如果离的很近了就必须考虑障碍物了
            if (dist < cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_force_inclusion_factor)
            {
                iter_obstacle->push_back(obst);
                continue;
            }
            // cut-off distance
            if (dist > cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_cutoff_factor) continue;
            // determine side (left or right) and assign obstacle if closer than the previous one // left
            if (cross2d(pose_orient, obst->getCentroid()) > 0)
            {
                if (dist < left_min_dist)
                {
                    left_min_dist = dist;
                    left_obstacle = obst;
                }
            }
            else
            {
                if (dist < right_min_dist)
                {
                    right_min_dist = dist;
                    right_obstacle = obst;
                }
            }
        }
        if (left_obstacle) iter_obstacle->push_back(left_obstacle);
        if (right_obstacle) iter_obstacle->push_back(right_obstacle);
        // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges
        if (i == 0)
        {
            ++iter_obstacle;
            continue;
        }
        // create obstacle edges 创建障碍物约束 // 上面的lambda表达式
        for (const ObstaclePtr obst : *iter_obstacle) create_edge(i, obst.get());
        ++iter_obstacle;
    }
}

void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier)
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == nullptr) return;
    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
    Eigen::Matrix<double, 2, 2> information_inflated;
    information_inflated(0, 0) = cfg_->optim.weight_obstacle * weight_multiplier;
    information_inflated(1, 1) = cfg_->optim.weight_inflation;
    information_inflated(0, 1) = information_inflated(1, 0) = 0;
    bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
    for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
    {
        // we handle dynamic obstacles differently below
        if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic())continue;
        int index;
        if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses()) index = teb_.sizePoses() / 2;
        else index = teb_.findClosestTrajectoryPose(*(obst->get()));
        // check if obstacle is outside index-range between start and goal
        // start and goal are fixed and findNearestBandpoint finds first or last conf if intersection point is outside the range
        if ((index <= 1) || (index > teb_.sizePoses() - 2)) continue;
        if (inflated)
        {
            EdgeInflatedObstacle *dist_bandpt_obst = new EdgeInflatedObstacle;
            dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index));
            dist_bandpt_obst->setInformation(information_inflated);
            dist_bandpt_obst->setParameters(*cfg_, obst->get());
            optimizer_->addEdge(dist_bandpt_obst);
        }
        else
        {
            EdgeObstacle *dist_bandpt_obst = new EdgeObstacle;
            dist_bandpt_obst->setVertex(0, teb_.PoseVertex(index));
            dist_bandpt_obst->setInformation(information);
            dist_bandpt_obst->setParameters(*cfg_, obst->get());
            optimizer_->addEdge(dist_bandpt_obst);
        }
        for (int neighbourIdx = 0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected / 2); neighbourIdx++)
        {
            if (index + neighbourIdx < teb_.sizePoses())
            {
                if (inflated)
                {
                    EdgeInflatedObstacle *dist_bandpt_obst_n_r = new EdgeInflatedObstacle;
                    dist_bandpt_obst_n_r->setVertex(0, teb_.PoseVertex(index + neighbourIdx));
                    dist_bandpt_obst_n_r->setInformation(information_inflated);
                    dist_bandpt_obst_n_r->setParameters(*cfg_, obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_r);
                }
                else
                {
                    EdgeObstacle *dist_bandpt_obst_n_r = new EdgeObstacle;
                    dist_bandpt_obst_n_r->setVertex(0, teb_.PoseVertex(index + neighbourIdx));
                    dist_bandpt_obst_n_r->setInformation(information);
                    dist_bandpt_obst_n_r->setParameters(*cfg_, obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_r);
                }
            }
            // needs to be casted to int to allow negative values
            if (index - neighbourIdx >= 0)
            {
                if (inflated)
                {
                    EdgeInflatedObstacle *dist_bandpt_obst_n_l = new EdgeInflatedObstacle;
                    dist_bandpt_obst_n_l->setVertex(0, teb_.PoseVertex(index - neighbourIdx));
                    dist_bandpt_obst_n_l->setInformation(information_inflated);
                    dist_bandpt_obst_n_l->setParameters(*cfg_, obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_l);
                }
                else
                {
                    EdgeObstacle *dist_bandpt_obst_n_l = new EdgeObstacle;
                    dist_bandpt_obst_n_l->setVertex(0, teb_.PoseVertex(index - neighbourIdx));
                    dist_bandpt_obst_n_l->setInformation(information);
                    dist_bandpt_obst_n_l->setParameters(*cfg_, obst->get());
                    optimizer_->addEdge(dist_bandpt_obst_n_l);
                }
            }
        }
    }
}

void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier)
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_obstacle == 0 || weight_multiplier == 0 || obstacles_ == NULL) return;
    Eigen::Matrix<double, 2, 2> information;
    information(0, 0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
    information(1, 1) = cfg_->optim.weight_dynamic_obstacle_inflation;
    information(0, 1) = information(1, 0) = 0;
    for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
    {
        if (!(*obst)->isDynamic()) continue;
        // 跳过第一个和最后一个位姿，因为他们是固定的
        // Skip first and last pose, as they are fixed
        double time = teb_.TimeDiff(0);
        for (int i = 1; i < teb_.sizePoses() - 1; ++i)
        {
            EdgeDynamicObstacle *dynobst_edge = new EdgeDynamicObstacle(time);
            dynobst_edge->setVertex(0, teb_.PoseVertex(i));
            dynobst_edge->setInformation(information);
            dynobst_edge->setParameters(*cfg_, obst->get());
            optimizer_->addEdge(dynobst_edge);
            // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1".
            time += teb_.TimeDiff(i);
        }
    }
}

void TebOptimalPlanner::AddEdgesViaPoints()
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_viapoint == 0 || via_points_ == NULL || via_points_->empty()) return;
    int start_pose_idx = 0;
    int n = teb_.sizePoses();
    // we do not have any degrees of freedom for reaching via-points
    if (n < 3)  return;
    for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it)
    {

        int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx);
        // skip a point to have a DOF inbetween for further via-points
        if (cfg_->trajectory.via_points_ordered) start_pose_idx = index + 2;
        // check if point conicides with goal or is located behind it
        // set to a pose before the goal, since we can move it away!
        if (index > n - 2) index = n - 2;
        // check if point coincides with start or is located before it
        if (index < 1)
        {
            // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later.
            if (cfg_->trajectory.via_points_ordered) index = 1;
            else
            {
                ROS_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
                continue; // skip via points really close or behind the current robot pose
            }
        }
        Eigen::Matrix<double, 1, 1> information;
        information.fill(cfg_->optim.weight_viapoint);
        EdgeViaPoint *edge_viapoint = new EdgeViaPoint;
        edge_viapoint->setVertex(0, teb_.PoseVertex(index));
        edge_viapoint->setInformation(information);
        edge_viapoint->setParameters(*cfg_, &(*vp_it));
        optimizer_->addEdge(edge_viapoint);
    }
}

void TebOptimalPlanner::AddEdgesVelocity()
{
    // non-holonomic robot
    if (cfg_->robot.max_vel_y == 0)
    {
        // if weight equals zero skip adding edges!
        if (cfg_->optim.weight_max_vel_x == 0 && cfg_->optim.weight_max_vel_theta == 0) return;
        int n = teb_.sizePoses();
        Eigen::Matrix<double, 2, 2> information;
        information(0, 0) = cfg_->optim.weight_max_vel_x;
        information(1, 1) = cfg_->optim.weight_max_vel_theta;
        information(0, 1) = 0.0;
        information(1, 0) = 0.0;
        for (int i = 0; i < n - 1; ++i)
        {
            EdgeVelocity *velocity_edge = new EdgeVelocity;
            velocity_edge->setVertex(0, teb_.PoseVertex(i));
            velocity_edge->setVertex(1, teb_.PoseVertex(i + 1));
            velocity_edge->setVertex(2, teb_.TimeDiffVertex(i));
            velocity_edge->setInformation(information);
            velocity_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(velocity_edge);
        }
    }
        // holonomic-robot
    else
    {
        // if weight equals zero skip adding edges!
        if (cfg_->optim.weight_max_vel_x == 0 && cfg_->optim.weight_max_vel_y == 0 &&
            cfg_->optim.weight_max_vel_theta == 0) return;
        int n = teb_.sizePoses();
        Eigen::Matrix<double, 3, 3> information;
        information.fill(0);
        information(0, 0) = cfg_->optim.weight_max_vel_x;
        information(1, 1) = cfg_->optim.weight_max_vel_y;
        information(2, 2) = cfg_->optim.weight_max_vel_theta;
        for (int i = 0; i < n - 1; ++i)
        {
            EdgeVelocityHolonomic *velocity_edge = new EdgeVelocityHolonomic;
            velocity_edge->setVertex(0, teb_.PoseVertex(i));
            velocity_edge->setVertex(1, teb_.PoseVertex(i + 1));
            velocity_edge->setVertex(2, teb_.TimeDiffVertex(i));
            velocity_edge->setInformation(information);
            velocity_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(velocity_edge);
        }
    }
}

void TebOptimalPlanner::AddEdgesAcceleration()
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_acc_lim_x == 0 && cfg_->optim.weight_acc_lim_theta == 0) return;
    int n = teb_.sizePoses();
    // non-holonomic robot
    if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0)
    {
        Eigen::Matrix<double, 2, 2> information;
        information.fill(0);
        information(0, 0) = cfg_->optim.weight_acc_lim_x;
        information(1, 1) = cfg_->optim.weight_acc_lim_theta;
        // check if an initial velocity should be taken into accound
        if (vel_start_.first)
        {
            EdgeAccelerationStart *acceleration_edge = new EdgeAccelerationStart;
            acceleration_edge->setVertex(0, teb_.PoseVertex(0));
            acceleration_edge->setVertex(1, teb_.PoseVertex(1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(0));
            acceleration_edge->setInitialVelocity(vel_start_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
        // now add the usual acceleration edge for each tuple of three teb poses
        for (int i = 0; i < n - 2; ++i)
        {
            EdgeAcceleration *acceleration_edge = new EdgeAcceleration;
            acceleration_edge->setVertex(0, teb_.PoseVertex(i));
            acceleration_edge->setVertex(1, teb_.PoseVertex(i + 1));
            acceleration_edge->setVertex(2, teb_.PoseVertex(i + 2));
            acceleration_edge->setVertex(3, teb_.TimeDiffVertex(i));
            acceleration_edge->setVertex(4, teb_.TimeDiffVertex(i + 1));
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
        // check if a goal velocity should be taken into accound
        if (vel_goal_.first)
        {
            EdgeAccelerationGoal *acceleration_edge = new EdgeAccelerationGoal;
            acceleration_edge->setVertex(0, teb_.PoseVertex(n - 2));
            acceleration_edge->setVertex(1, teb_.PoseVertex(n - 1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(teb_.sizeTimeDiffs() - 1));
            acceleration_edge->setGoalVelocity(vel_goal_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
    }
        // holonomic robot
    else
    {
        Eigen::Matrix<double, 3, 3> information;
        information.fill(0);
        information(0, 0) = cfg_->optim.weight_acc_lim_x;
        information(1, 1) = cfg_->optim.weight_acc_lim_y;
        information(2, 2) = cfg_->optim.weight_acc_lim_theta;
        // check if an initial velocity should be taken into accound
        if (vel_start_.first)
        {
            EdgeAccelerationHolonomicStart *acceleration_edge = new EdgeAccelerationHolonomicStart;
            acceleration_edge->setVertex(0, teb_.PoseVertex(0));
            acceleration_edge->setVertex(1, teb_.PoseVertex(1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(0));
            acceleration_edge->setInitialVelocity(vel_start_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
        // now add the usual acceleration edge for each tuple of three teb poses
        for (int i = 0; i < n - 2; ++i)
        {
            EdgeAccelerationHolonomic *acceleration_edge = new EdgeAccelerationHolonomic;
            acceleration_edge->setVertex(0, teb_.PoseVertex(i));
            acceleration_edge->setVertex(1, teb_.PoseVertex(i + 1));
            acceleration_edge->setVertex(2, teb_.PoseVertex(i + 2));
            acceleration_edge->setVertex(3, teb_.TimeDiffVertex(i));
            acceleration_edge->setVertex(4, teb_.TimeDiffVertex(i + 1));
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
        // check if a goal velocity should be taken into accound
        if (vel_goal_.first)
        {
            EdgeAccelerationHolonomicGoal *acceleration_edge = new EdgeAccelerationHolonomicGoal;
            acceleration_edge->setVertex(0, teb_.PoseVertex(n - 2));
            acceleration_edge->setVertex(1, teb_.PoseVertex(n - 1));
            acceleration_edge->setVertex(2, teb_.TimeDiffVertex(teb_.sizeTimeDiffs() - 1));
            acceleration_edge->setGoalVelocity(vel_goal_.second);
            acceleration_edge->setInformation(information);
            acceleration_edge->setTebConfig(*cfg_);
            optimizer_->addEdge(acceleration_edge);
        }
    }
}

void TebOptimalPlanner::AddEdgesTimeOptimal()
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_optimaltime == 0) return;
    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_optimaltime);
    for (int i = 0; i < teb_.sizeTimeDiffs(); ++i)
    {
        EdgeTimeOptimal *timeoptimal_edge = new EdgeTimeOptimal;
        timeoptimal_edge->setVertex(0, teb_.TimeDiffVertex(i));
        timeoptimal_edge->setInformation(information);
        timeoptimal_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(timeoptimal_edge);
    }
}

void TebOptimalPlanner::AddEdgesShortestPath()
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_shortest_path == 0) return;
    Eigen::Matrix<double, 1, 1> information;
    information.fill(cfg_->optim.weight_shortest_path);
    for (int i = 0; i < teb_.sizePoses() - 1; ++i)
    {
        EdgeShortestPath *shortest_path_edge = new EdgeShortestPath;
        shortest_path_edge->setVertex(0, teb_.PoseVertex(i));
        shortest_path_edge->setVertex(1, teb_.PoseVertex(i + 1));
        shortest_path_edge->setInformation(information);
        shortest_path_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(shortest_path_edge);
    }
}

void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_kinematics_nh == 0 && cfg_->optim.weight_kinematics_forward_drive == 0) return;
    // create edge for satisfiying kinematic constraints
    Eigen::Matrix<double, 2, 2> information_kinematics;
    information_kinematics.fill(0.0);
    information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
    information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive;
    // ignore twiced start only
    for (int i = 0; i < teb_.sizePoses() - 1; i++)
    {
        auto *kinematics_edge = new EdgeKinematicsDiffDrive;
        kinematics_edge->setVertex(0, teb_.PoseVertex(i));
        kinematics_edge->setVertex(1, teb_.PoseVertex(i + 1));
        kinematics_edge->setInformation(information_kinematics);
        kinematics_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(kinematics_edge);
    }
}

void TebOptimalPlanner::AddEdgesKinematicsCarlike()
{
    // if weight equals zero skip adding edges!
    if (cfg_->optim.weight_kinematics_nh == 0 && cfg_->optim.weight_kinematics_turning_radius == 0) return;
    // create edge for satisfiying kinematic constraints
    Eigen::Matrix<double, 2, 2> information_kinematics;
    information_kinematics.fill(0.0);
    information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
    information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius;
    // ignore twiced start only
    for (int i = 0; i < teb_.sizePoses() - 1; i++)
    {
        EdgeKinematicsCarlike *kinematics_edge = new EdgeKinematicsCarlike;
        kinematics_edge->setVertex(0, teb_.PoseVertex(i));
        kinematics_edge->setVertex(1, teb_.PoseVertex(i + 1));
        kinematics_edge->setInformation(information_kinematics);
        kinematics_edge->setTebConfig(*cfg_);
        optimizer_->addEdge(kinematics_edge);
    }
}

void TebOptimalPlanner::AddEdgesPreferRotDir()
{
    //TODO(roesmann): Note, these edges can result in odd predictions, in particular
    //                we can observe a substantional mismatch between open- and closed-loop planning
    //                leading to a poor control performance.
    //                At the moment, we keep these functionality for oscillation recovery:
    //                Activating the edge for a short time period might not be crucial and
    //                could move the robot to a new oscillation-free state.
    //                This needs to be analyzed in more detail!
    // if weight equals zero skip adding edges!
    if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0) return;
    if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
    {
        ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
        return;
    }
    // create edge for satisfiying kinematic constraints
    Eigen::Matrix<double, 1, 1> information_rotdir;
    information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);
    // currently: apply to first 3 rotations
    for (int i = 0; i < teb_.sizePoses() - 1 && i < 3; ++i)
    {
        EdgePreferRotDir *rotdir_edge = new EdgePreferRotDir;
        rotdir_edge->setVertex(0, teb_.PoseVertex(i));
        rotdir_edge->setVertex(1, teb_.PoseVertex(i + 1));
        rotdir_edge->setInformation(information_rotdir);
        if (prefer_rotdir_ == RotType::left) rotdir_edge->preferLeft();
        else if (prefer_rotdir_ == RotType::right) rotdir_edge->preferRight();
        optimizer_->addEdge(rotdir_edge);
    }
}

void TebOptimalPlanner::AddEdgesVelocityObstacleRatio()
{
    Eigen::Matrix<double, 2, 2> information;
    information(0, 0) = cfg_->optim.weight_velocity_obstacle_ratio;
    information(1, 1) = cfg_->optim.weight_velocity_obstacle_ratio;
    information(0, 1) = information(1, 0) = 0;
    auto iter_obstacle = obstacles_per_vertex_.begin();
    for (int index = 0; index < teb_.sizePoses() - 1; ++index)
    {
        for (const ObstaclePtr obstacle : (*iter_obstacle++))
        {
            EdgeVelocityObstacleRatio *edge = new EdgeVelocityObstacleRatio;
            edge->setVertex(0, teb_.PoseVertex(index));
            edge->setVertex(1, teb_.PoseVertex(index + 1));
            edge->setVertex(2, teb_.TimeDiffVertex(index));
            edge->setInformation(information);
            edge->setParameters(*cfg_, obstacle.get());
            optimizer_->addEdge(edge);
        }
    }
}

bool TebOptimalPlanner::hasDiverged() const
{
    // 如果发散性检测是false，返回false
    // Early returns if divergence detection is not active
    if (!cfg_->recovery.divergence_detection_enable) return false;
    auto stats_vector = optimizer_->batchStatistics();
    // No statistics yet 还没有数据
    if (stats_vector.empty()) return false;
    // Grab the statistics of the final iteration 获取最后一次迭代的数据
    const auto last_iter_stats = stats_vector.back();
    return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared;
}

void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale,
                                           bool alternative_time_cost)
{
    // check if graph is empty/exist  -> important if function is called between buildGraph and optimizeGraph/clearGraph
    bool graph_exist_flag(false);
    if (optimizer_->edges().empty() && optimizer_->vertices().empty())
    {
        // here the graph is build again, for time efficiency make sure to call this function
        // between buildGraph and Optimize (deleted), but it depends on the application
        buildGraph();
        optimizer_->initializeOptimization();
    }
    else graph_exist_flag = true;
    optimizer_->computeInitialGuess();
    cost_ = 0;
    // true 代表cost累加方式为  \f$ \sum_i \Delta T_i \f$
    if (alternative_time_cost)
    {
        cost_ += teb_.getSumOfAllTimeDiffs();
        // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
        // since we are using an AutoResize Function with hysteresis.
    }
    // 现在开始累加所有的代价。因为没有存储边的指针，所有要检查每个边是不是nullptr
    // now we need pointers to all edges -> calculate error for each edge-type
    // since we aren't storing edge pointers, we need to check every edge
    for (auto it : optimizer_->activeEdges())
    {
        double cur_cost = it->chi2();
        if (dynamic_cast<EdgeObstacle *>(it) != nullptr
            || dynamic_cast<EdgeInflatedObstacle *>(it) != nullptr
            || dynamic_cast<EdgeDynamicObstacle *>(it) != nullptr) cur_cost *= obst_cost_scale;
        else if (dynamic_cast<EdgeViaPoint *>(it) != nullptr) cur_cost *= viapoint_cost_scale;
            // skip these edges if alternative_time_cost is active  如果alternative_time_cost是true, 该循环的代价被忽略
        else if (dynamic_cast<EdgeTimeOptimal *>(it) != nullptr && alternative_time_cost) continue;
        cost_ += cur_cost;
    }
    // delete temporary created graph 删除临时创建的图
    if (!graph_exist_flag) clearGraph();
}

void TebOptimalPlanner::extractVelocity(const PoseSE2 &pose1, const PoseSE2 &pose2, double dt, double &vx, double &vy,
                                   double &omega) const
{
    if (dt == 0)
    {
        vx = 0;
        vy = 0;
        omega = 0;
        return;
    }
    Eigen::Vector2d deltaS = pose2.position() - pose1.position();
    // nonholonomic robot
    if (cfg_->robot.max_vel_y == 0)
    {
        Eigen::Vector2d conf1dir(cos(pose1.theta()), sin(pose1.theta()));
        // translational velocity 线速度
        double dir = deltaS.dot(conf1dir);
        vx = (double) g2o::sign(dir) * deltaS.norm() / dt;
        vy = 0;
    }
        // holonomic robot
    else
    {
        // transform pose 2 into the current robot frame (pose1)
        // for velocities only the rotation of the direction vector is necessary.
        // (map->pose1-frame: inverse 2d rotation matrix)
        double cos_theta1 = std::cos(pose1.theta());
        double sin_theta1 = std::sin(pose1.theta());
        double p1_dx = cos_theta1 * deltaS.x() + sin_theta1 * deltaS.y();
        double p1_dy = -sin_theta1 * deltaS.x() + cos_theta1 * deltaS.y();
        vx = p1_dx / dt;
        vy = p1_dy / dt;
    }
    // rotational velocity 角速度计算
    double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
    omega = orientdiff / dt;
}

bool TebOptimalPlanner::getVelocityCommand(double &vx, double &vy, double &omega, int look_ahead_poses) const
{
    if (teb_.sizePoses() < 2)
    {
        ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. "
                  "Make sure to init and optimize/plan the trajectory fist.");
        vx = 0;
        vy = 0;
        omega = 0;
        return false;
    }
    look_ahead_poses = std::max(1, std::min(look_ahead_poses,
            teb_.sizePoses() - 1 - cfg_->trajectory.prevent_look_ahead_poses_near_goal));
    double dt = 0.0;
    for (int counter = 0; counter < look_ahead_poses; ++counter)
    {
        dt += teb_.TimeDiff(counter);
        // TODO: change to look-ahead time? Refine trajectory?
        if (dt >= cfg_->trajectory.dt_ref * look_ahead_poses)
        {
            look_ahead_poses = counter + 1;
            break;
        }
    }
    if (dt <= 0)
    {
        ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");
        vx = 0;
        vy = 0;
        omega = 0;
        return false;
    }
    // 从第一个位姿点和look_ahead_poses提取速度
    // Get velocity from the first two configurations
    extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
    return true;
}

void TebOptimalPlanner::getVelocityProfile(std::vector<geometry_msgs::Twist> &velocity_profile) const
{
    int n = teb_.sizePoses();
    velocity_profile.resize(n + 1);
    // start velocity  起始点的速度
    velocity_profile.front().linear.z = 0;
    velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0;
    velocity_profile.front().linear.x = vel_start_.second.linear.x;
    velocity_profile.front().linear.y = vel_start_.second.linear.y;
    velocity_profile.front().angular.z = vel_start_.second.angular.z;
    for (int i = 1; i < n; ++i)
    {
        velocity_profile[i].linear.z = 0;
        velocity_profile[i].angular.x = velocity_profile[i].angular.y = 0;
        extractVelocity(teb_.Pose(i - 1), teb_.Pose(i), teb_.TimeDiff(i - 1), velocity_profile[i].linear.x,
                        velocity_profile[i].linear.y, velocity_profile[i].angular.z);
    }
    // goal velocity 目标点的速度
    velocity_profile.back().linear.z = 0;
    velocity_profile.back().angular.x = velocity_profile.back().angular.y = 0;
    velocity_profile.back().linear.x = vel_goal_.second.linear.x;
    velocity_profile.back().linear.y = vel_goal_.second.linear.y;
    velocity_profile.back().angular.z = vel_goal_.second.angular.z;
}

void TebOptimalPlanner::getFullTrajectory(std::vector<TrajectoryPointMsg> &trajectory) const
{
    int n = teb_.sizePoses();
    trajectory.resize(n);
    if (n == 0) return;
    double curr_time = 0;
    // start 起始点
    TrajectoryPointMsg &start = trajectory.front();
    teb_.Pose(0).toPoseMsg(start.pose);
    start.velocity.linear.z = 0;
    start.velocity.angular.x = start.velocity.angular.y = 0;
    start.velocity.linear.x = vel_start_.second.linear.x;
    start.velocity.linear.y = vel_start_.second.linear.y;
    start.velocity.angular.z = vel_start_.second.angular.z;
    start.time_from_start.fromSec(curr_time);
    curr_time += teb_.TimeDiff(0);
    // intermediate points 中间点
    for (int i = 1; i < n - 1; ++i)
    {
        TrajectoryPointMsg &point = trajectory[i];
        teb_.Pose(i).toPoseMsg(point.pose);
        point.velocity.linear.z = 0;
        point.velocity.angular.x = point.velocity.angular.y = 0;
        double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2;
        extractVelocity(teb_.Pose(i - 1), teb_.Pose(i), teb_.TimeDiff(i - 1), vel1_x, vel1_y, omega1);
        extractVelocity(teb_.Pose(i), teb_.Pose(i + 1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2);
        point.velocity.linear.x = 0.5 * (vel1_x + vel2_x);
        point.velocity.linear.y = 0.5 * (vel1_y + vel2_y);
        point.velocity.angular.z = 0.5 * (omega1 + omega2);
        point.time_from_start.fromSec(curr_time);
        curr_time += teb_.TimeDiff(i);
    }
    // goal 目标点
    TrajectoryPointMsg &goal = trajectory.back();
    teb_.BackPose().toPoseMsg(goal.pose);
    goal.velocity.linear.z = 0;
    goal.velocity.angular.x = goal.velocity.angular.y = 0;
    goal.velocity.linear.x = vel_goal_.second.linear.x;
    goal.velocity.linear.y = vel_goal_.second.linear.y;
    goal.velocity.angular.z = vel_goal_.second.angular.z;
    goal.time_from_start.fromSec(curr_time);
}

bool TebOptimalPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel *costmap_model,
                                             const std::vector<geometry_msgs::Point> &footprint_spec,
                                             double inscribed_radius, double circumscribed_radius,
                                             int look_ahead_idx, double feasibility_check_lookahead_distance)
{
    if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses()) look_ahead_idx = teb().sizePoses() - 1;
    if (feasibility_check_lookahead_distance > 0)
    {
        for (int i = 1; i < teb().sizePoses(); ++i)
        {
            double pose_distance = std::hypot(teb().Pose(i).x() - teb().Pose(0).x(),
                                              teb().Pose(i).y() - teb().Pose(0).y());
            if (pose_distance > feasibility_check_lookahead_distance)
            {
                look_ahead_idx = i - 1;
                break;
            }
        }
    }
    for (int i = 0; i <= look_ahead_idx; ++i)
    {
        if (costmap_model->footprintCost(teb().Pose(i).x(), teb().Pose(i).y(), teb().Pose(i).theta(),
                                         footprint_spec, inscribed_radius, circumscribed_radius) == -1)
        {
            if (visualization_) visualization_->publishInfeasibleRobotPose(teb().Pose(i), *cfg_->robot_model, footprint_spec);
            return false;
        }
        // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold
        // and interpolates in that case.
        // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)!
        if (i < look_ahead_idx)
        {
            double delta_rot = g2o::normalize_theta(g2o::normalize_theta(teb().Pose(i + 1).theta()) -
                                                    g2o::normalize_theta(teb().Pose(i).theta()));
            Eigen::Vector2d delta_dist = teb().Pose(i + 1).position() - teb().Pose(i).position();
            if (fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular ||
                delta_dist.norm() > inscribed_radius)
            {
                int n_additional_samples = std::max(
                        std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular),
                        std::ceil(delta_dist.norm() / inscribed_radius)) - 1;
                PoseSE2 intermediate_pose = teb().Pose(i);
                for (int step = 0; step < n_additional_samples; ++step)
                {
                    intermediate_pose.position() =
                            intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0);
                    intermediate_pose.theta() = g2o::normalize_theta(
                            intermediate_pose.theta() + delta_rot / (n_additional_samples + 1.0));
                    if (costmap_model->footprintCost(
                            intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(),
                            footprint_spec, inscribed_radius, circumscribed_radius) == -1)
                    {
                        if (visualization_)
                        {
                            visualization_->publishInfeasibleRobotPose(intermediate_pose, *cfg_->robot_model, footprint_spec);
                        }
                        return false;
                    }
                }
            }
        }
    }
    return true;
}

} // namespace teb_local_planner
